package es.uji.viselab.robot.links.fanuc;

import java.io.ByteArrayInputStream;
import java.io.IOException;
import java.text.DecimalFormat;
import java.text.DecimalFormatSymbols;
import java.text.ParseException;

import javax.xml.parsers.DocumentBuilder;
import javax.xml.parsers.DocumentBuilderFactory;
import javax.xml.parsers.ParserConfigurationException;

import org.omg.CORBA.portable.InputStream;
import org.w3c.dom.DOMException;
import org.w3c.dom.Document;
import org.w3c.dom.Element;
import org.w3c.dom.Node;
import org.xml.sax.SAXException;

import es.uji.viselab.math.Joints;
import es.uji.viselab.math.PointXYZ;
import es.uji.viselab.math.RobotToolLocation;
import es.uji.viselab.robot.RobotException;

/**
 * 
 * Stores the response of the robot server. As the robot is a REST server and it
 * returns values in XML this class is responsible to store that XML, but also
 * provide functionality to transform the XML in other class ( as Joints)
 * 
 */
public class RobotResponse {

	private String xml;
	private String message;

	public RobotResponse(String s) {
		xml = s;
	}

	public RobotResponse() {

	}

	public String getXml() {
		return xml;
	}

	public void setXml(String xml) {
		this.xml = xml;
	}

	public String getMessage() throws RobotException {

		try {
			DocumentBuilderFactory factory =DocumentBuilderFactory.newInstance();
			DocumentBuilder builder = factory.newDocumentBuilder();
			//xml="<RobotResponse><message>Pong</message></RobotResponse>";
			//xml="<book><title>Fun Software</title><author>Jakob Jenkov</author><ISBN>0123456789</ISBN></book>";
			ByteArrayInputStream stream = new ByteArrayInputStream(xml.getBytes());
			Document document = builder.parse(stream);
			Node node = document.getFirstChild().getFirstChild();
			String text = document.getElementsByTagName("message").item(0).getTextContent();
			return text;
		} catch (SAXException e) {
			throw new RobotException(e.getMessage());
		} catch (IOException e) {
			throw new RobotException(e.getMessage());
		} catch (ParserConfigurationException e) {
			throw new RobotException(e.getMessage());
		}

	}


	private int stringToInteger(String value) throws RobotException {
		return stringToLong(value).intValue();
	}

	private Long stringToLong(String stringValue) throws RobotException {
		Number numberValue;
		Long longValue;
		DecimalFormatSymbols tailoredFormat = new DecimalFormatSymbols();
		tailoredFormat.setDecimalSeparator(',');
		String mask = "#########,#############";
		DecimalFormat df = new DecimalFormat(mask, tailoredFormat);
		try {
			numberValue = df.parse(stringValue);
		} catch (ParseException e) {
			throw new RobotException(e.getMessage());
		}
		longValue = numberValue.longValue();
		return longValue;
	}
	
	public RobotToolLocation toToolLocation() throws RobotException {

		Element node;
		String xml = this.getXml();
		PointXYZ locationxyz = new PointXYZ();
		PointXYZ orientationxyz = new PointXYZ();
		RobotToolLocation rtl = new RobotToolLocation();

		try {
			node = DocumentBuilderFactory.newInstance().newDocumentBuilder()
					.parse(new ByteArrayInputStream(xml.getBytes())).getDocumentElement();
			locationxyz.setX(stringToLong(node.getElementsByTagName("X").item(0).getFirstChild()
					.getNodeValue()));
			locationxyz.setY(stringToLong(node.getElementsByTagName("Y").item(0).getFirstChild()
					.getNodeValue()));
			locationxyz.setZ(stringToLong(node.getElementsByTagName("Z").item(0).getFirstChild()
					.getNodeValue()));
			orientationxyz.setX(stringToLong(node.getElementsByTagName("W").item(0).getFirstChild()
					.getNodeValue()));
			orientationxyz.setY(stringToLong(node.getElementsByTagName("P").item(0).getFirstChild()
					.getNodeValue()));
			orientationxyz.setZ(stringToLong(node.getElementsByTagName("R").item(0).getFirstChild()
					.getNodeValue()));
			rtl = new RobotToolLocation(locationxyz, orientationxyz);
			return rtl;

		} catch (SAXException e) {
			throw new RobotException(e.getMessage());
		} catch (IOException e) {
			throw new RobotException(e.getMessage());
		} catch (ParserConfigurationException e) {
			throw new RobotException(e.getMessage());
		}

	}
	
	/*
	 * @param RobotResponse
	 * 
	 * @return Joints
	 * 
	 * @throws RobotException Convert a response of the robot that has the
	 * convenient xml message to the Joints object, if the xml doccument is not
	 * adequate, this method throws the RobotException
	 */
	public Joints toJoints() throws RobotException {

		Element node;
		String xml = this.getXml();
		Joints joints = new Joints();

		try {
			node = DocumentBuilderFactory.newInstance().newDocumentBuilder()
					.parse(new ByteArrayInputStream(xml.getBytes())).getDocumentElement();
			for (int i = 1; i < 7; i++) {
				joints.setJoint(i, stringToInteger(node.getElementsByTagName("joint" + i).item(0)
						.getFirstChild().getNodeValue()));
			}
			return joints;

		} catch (SAXException e) {
			throw new RobotException(e.getMessage());
		} catch (IOException e) {
			throw new RobotException(e.getMessage());
		} catch (ParserConfigurationException e) {
			throw new RobotException(e.getMessage());
		} catch (DOMException e) {
			throw new RobotException(e.getMessage());
		}

	}

	
}
